/*
 * stepper.c
 *
 *  Created on: Jan 28, 2025
 *      Author: wzx05
 */

#include "stm32f1xx_hal.h"     // 基础HAL库
#include "stm32f1xx_hal_tim.h" // 定时器HAL库
#include <math.h>

#include <Motor/shift_reg.h>
#include <Motor/stepper_cfg.h>
#include <Motor/stepper_driver.h>
#include <Motor/stepper_group.h>
#include <Motor/stepper.h>

// 保存所有电机的运行状态：二维数组 [组号][电机号]
static MotorState motor_state[MOTOR_GROUPS][MOTORS_PER_GROUP];

/**
 * @brief 在定时器中断中调用：更新所有电机状态，并组合各电机相位数据后写入移位寄存器组
 */
void Stepper_UpdateAllGroups(void) {
    for (uint8_t g = 0; g < MOTOR_GROUPS; g++) {
        uint16_t group_data = 0;
        for (uint8_t m = 0; m < MOTORS_PER_GROUP; m++) {
            MotorState* motor = &motor_state[g][m];
            uint8_t phase = motor->spec.idle_pattern; // 默认放到电机Idle Pattern

            // 如果目标步数与当前步数不一致，则更新一步
            if (motor->current_steps != motor->target_steps) {
                int8_t dir = (motor->target_steps > motor->current_steps) ? 1 : -1;
                motor->current_steps += dir;
                // 更新相位索引（注意防止负值）
                motor->phase_index = (motor->phase_index + dir + motor->spec.phase_count) % motor->spec.phase_count;
                // 获取当前相位值（只取低4位有效数据）
                phase = motor->spec.phase_seq[motor->phase_index];
                //printf("Group %d Motor %d: Steps = %d, Phase = 0x%X\n", g, m, motor->current_steps, phase);
            }

            // 将当前电机的目标脉冲配置到组寄存器
            group_data |= (phase & 0x0F) << (BITS_PER_MOTOR * m);
        }

        // 整组下发
        //printf("Stepper_WriteGroup: Group %d Data = 0x%X\n", g, group_data);
        Stepper_WriteGroupAsync(g, group_data);
        //Stepper_WriteGroupSync(g, group_data);

    }
}

/**
 * @brief 定时器中断调用：更新所有电机组配套GPIO的输出状态（用于非阻塞控制GPIO脉宽）
 */
void Stepper_UpdateAllGroupTransfer(void) {
    for (uint8_t g = 0; g < MOTOR_GROUPS; g++) {
    	Stepper_UpdateGroupTransfer(g);
    }
}

/**
 * @brief 将角度转换为步数。考虑到电机可能处于半步模式，
 *        则有效步数应乘以2。
 * @param spec 指向电机规格参数的指针
 * @param degrees 目标角度（单位：度）
 * @return 计算得到的步数
 */
static int32_t degrees_to_steps(const MotorSpec* spec, float degrees) {
    // 根据驱动模式确定有效步数：如果是半步模式，则原始步数翻倍
    uint16_t effective_steps = spec->steps_per_rev;
    if (spec->step_mode == 1) { // 1表示半步模式
        effective_steps *= 2;
    }
    // 计算每度对应的步数：注意 gear_ratio 为减速比
    float steps_per_degree = (effective_steps * spec->gear_ratio) / 360.0f;
    return (int32_t)roundf(degrees * steps_per_degree);
}

/**
 * @brief 指令：让指定电机转到某一绝对角度
 */
void Stepper_MoveToAngle(uint8_t group, uint8_t motor, float degrees) {
    if (group >= MOTOR_GROUPS || motor >= MOTORS_PER_GROUP) return;
    MotorState* m = &motor_state[group][motor];
    m->target_steps = degrees_to_steps(&m->spec, degrees);
}

/**
 * @brief 系统全局初始化
 *
 * 初始化所有电机的状态及（如果需要）各组的移位寄存器
 */
void Stepper_GlobalInit(const MotorSpec* motor_spec) {
    // 对每个组的每个电机进行初始化（这里可设置默认的电机型号）
    for (uint8_t g = 0; g < MOTOR_GROUPS; g++) {
        // 初始化移位寄存器组
        Stepper_InitGroup(g);
        for (uint8_t m = 0; m < MOTORS_PER_GROUP; m++) {
            // 默认初始化：这里将所有电机设置为28BYJ-48型号，后续可通过 Stepper_ReconfigMotor() 修改
            motor_state[g][m].spec = MOTOR_24BYJ48;
            motor_state[g][m].current_steps = 0;
            motor_state[g][m].target_steps = 0;
            motor_state[g][m].phase_index = 0;
            motor_state[g][m].is_homing = false;
        }
        Stepper_EnableGroup(g, ENABLE);
    }
}



/* 其余接口例如 Stepper_Rotate、Stepper_Home、Stepper_IsBusy、Stepper_GetCurrentSteps/Angle
   可根据项目需求补充实现 */
